/**
  ******************************************************************************
  * @file    img processing2.c
  * @author  Amaseing team
  * @version V1.0.0
  * @date    21-november-2013
  * @brief   Img processing2
  *         
  ******************************************************************************  
  */ 
/* Private includes -----------------------------------------------------------*/
#include "img processing2.h"
#include "com.h" // remove this sometime after debugging
#include "led.h"
#include <stdio.h>
#include "motor.h"

/* Private prototypes ---------------------------------------------------------*/
// img process 2: Object detection
// Passes an image for processing. After calling this, the contents of img are modified.
enum eOBJECTS imgP2ObjRecognition(image_t* img, enum eCOLOR color);

/* Private defines -----------------------------------------------------------*/
/* Functions -----------------------------------------------------------------*/
void imgP2Init(void) {
	// Whatever it is that needs to be initialized.
	uint32_t i=0;
	
	SET_LED(LED_OBJ1);
	SET_LED(LED_OBJ2);
	SET_LED(LED_OBJ3);
	for(i=0;i<16800000; i++);
	RESET_LED(LED_OBJ1);
	RESET_LED(LED_OBJ2);
	RESET_LED(LED_OBJ3);
}

// img process 2: Color detection
// Passes an image for processing. After calling this, the contents of img are modified.
void imgP2(image_t* img) {
	static colorobject_t colorObjects[COLOR_SIZE];
	uint32_t color = 0, ystart = 0, xmean = 0, xstart = 0;
	
	RESET_LED(LED_OBJ1);
	RESET_LED(LED_OBJ2);
	RESET_LED(LED_OBJ3);
	
	img = camGetFrame();
	if(img == NULL)
		return;
				
	// Enhancement
	vCalculateMeanOfThreeRows(img);
	
	// First we scan for colour.
	if(iScanRowForColors(img, colorObjects, 3)){ //scan max 3 colors
		//For now, we use only the color in colorObjects[0]
		if((colorObjects[0].xstop - colorObjects[0].xstart) <= IMP2_WIDTH_MAX) { //breedte past binnen grens?
			color = colorObjects[0].color;
			xstart = colorObjects[0].xstart-IMP2_WIDTH_OFFSET;
			ystart = colorObjects[0].ystart;
			xmean = (colorObjects[0].xstop + colorObjects[0].xstart)/2;
			
			//check xmean is multiple of 4 and within cam range
			if((xmean+(IMP2_CAPTURE_WIDTH/2)) > CAM_WIDTH)
						xmean = CAM_WIDTH-(IMP2_CAPTURE_WIDTH/2);
			if(xmean>3){
				while(xmean%4) xmean--;
			}
			else
				xmean = 0;
			
			//find the start y position
			//capture line (4x160)
			ystart = (ystart-160);
			camSetROI(xmean,ystart,4,320);					
			img = camGetFrame();
			if(img == NULL){ //Data capture error
				camSetROI(xmean,ystart,4,320);
				img = camGetFrame();
				if(img == NULL) //Wrong ROI
					return;
			}
			
			if(iScanColumnForColors(img, (enum eCOLOR)color, &colorObjects[0])){
				if((colorObjects[0].ystart - colorObjects[0].ystart) <= IMP2_HEIGHT_MAX) { //hoogte past binnen grens
					ystart = ystart + (colorObjects[0].ystart) - IMP2_HEIGHT_OFFSET;
					
					//check xstart is multiple of 4 and within cam range
					if((xstart+160) > CAM_WIDTH)
						xstart = CAM_WIDTH-160;
					if(xstart>3){
						while(xstart%4) xstart--;
					}
					else
						xstart = 0;						
					
					//capture small picture (IMP2_CAPTURE_WIDTH,IMP2_CAPTURE_HEIGHT)
					camSetROI(xstart,(ystart),IMP2_CAPTURE_WIDTH,IMP2_CAPTURE_HEIGHT);		
					img = camGetFrame();			
					if(img == NULL){ //Data capture error
						camSetROI(xstart,(ystart),IMP2_CAPTURE_WIDTH,IMP2_CAPTURE_HEIGHT);
						img = camGetFrame();
						if(img == NULL) //Wrong ROI
							return;
					}
					
					colorObjects[0].object = imgP2ObjRecognition(img,(enum eCOLOR)color);

					//DEBUG printf, remove later
					/*
					char array2[50] = {'\0'};
					switch(colorObjects[0].object)
					{
						case OBJ_SQUARE:
							snprintf(array2,sizeof(array2)-1,"OBJ = SQUARE\n");
							break;
						case OBJ_TRIANGLE:
							snprintf(array2,sizeof(array2)-1,"OBJ = TRIANGLE\n");
							break;
						case OBJ_BAR:
							snprintf(array2,sizeof(array2)-1,"OBJ = BAR\n");
							break;
						default:
							snprintf(array2,sizeof(array2)-1,"OBJ = UNKNOWN\n");
							break;
					}
					
					int i=0;
					while(array2[i] != '\0'){
						USART_SendData(USART2, array2[i]);
						while(USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET);
						i++;
					}
					*/
					//END DEBUG
					comWrite(TYPE_OBJECT_FOUND, (char*)&colorObjects[0], sizeof(colorObjects[0]));
					
					if(color == COLOR_RED)
						SET_LED(LED_OBJ1);
					if(color == COLOR_GREEN)
						SET_LED(LED_OBJ2);
					if(color == COLOR_BLUE)
						SET_LED(LED_OBJ3);
				
					if(colorObjects[0].object != OBJ_UNKNOWN){
						switch(colorObjects[0].object)
						{
							case OBJ_SQUARE:
								motorPointTurn(TURN90L);
								break;
							case OBJ_TRIANGLE:
								motorPointTurn(TURN180R);
								break;
							case OBJ_BAR:
								motorPointTurn(TURN90R);
								break;
							default:
								break;
						}
						motorWaitWhileDriving();
						
						switch(colorObjects[0].color)
						{
							case COLOR_GREEN:
								motorDriveDistance(FORWARD, DIST1CM);
								break;
							case COLOR_RED:
								motorDriveDistance(FORWARD, DIST2CM);
								break;
							case COLOR_BLUE:
								motorDriveDistance(FORWARD,DIST5CM);
								break;
							default:
								break;
						}
					}
					else{ //OBJ unknown
						if(colorObjects[0].xstart < 200 && colorObjects[0].xstop < 300){
							//turn 45 degrees left
							//motorSetSpeed(-33, 33, 866/2);
							motorDriveDistance(BACKWARD,DIST1CM);
							motorWaitWhileDriving();
							
							motorPointTurn(TURN90L);
							motorWaitWhileDriving();
							motorDriveDistance(FORWARD,DIST2CM);
							motorWaitWhileDriving();
							motorPointTurn(TURN90R);
							
							motorWaitWhileDriving();
							motorDriveDistance(FORWARD,DIST1CM);
							
						}
						else if(colorObjects[0].xstart > 1080 && colorObjects[0].xstop > 980){
							//turn 45 degrees right
							//motorSetSpeed(33, -33, 866/2);
							motorDriveDistance(BACKWARD,DIST1CM);
							motorWaitWhileDriving();
							
							motorPointTurn(TURN90R);
							motorWaitWhileDriving();
							motorDriveDistance(FORWARD,DIST2CM);
							motorWaitWhileDriving();
							motorPointTurn(TURN90L);
							
							motorWaitWhileDriving();
							motorDriveDistance(FORWARD,DIST1CM);
						}
						else if((colorObjects[0].ystop - colorObjects[0].ystart) < 25){
							motorDriveDistance(FORWARD,DIST2CM);
						}
						else if((colorObjects[0].ystop - colorObjects[0].ystart) < 144){
							motorDriveDistance(BACKWARD,DIST2CM);
						}
						else{
							motorDriveDistance(BACKWARD,DIST2CM);
						}
					}

					//SET_LED(LED_CAPTURE); //set capture led
					
					//RESET_LED(LED_OBJ1);
					//RESET_LED(LED_OBJ2);
					//RESET_LED(LED_OBJ3);
				}
				else{ //object would not fit in 144pix y-as
					RESET_LED(LED_CAPTURE); //reset capture led
					motorDriveDistance(BACKWARD,DIST2CM);
				}
			}
			//Set settings back to orginal
			camSetROI(0,512,1280,3); 
		}
		else{ //object would not fit in 144pix x-as
			motorDriveDistance(BACKWARD,DIST2CM);
		}
	}
	else{RESET_LED(LED_CAPTURE);} //reset capture led
}

enum eOBJECTS imgP2ObjRecognition(image_t* img, enum eCOLOR color){
	uint32_t blobs = 0, M00 = 0, object = OBJ_UNKNOWN, blobsize = 0;
	uint8_t Xc, Yc;
	double Q1=0, Q2=0,N20=0, N02=0, N11=0;

	//Threshold the color
	vThresholdYUVColor(img, color);

	//Remove the border blobs
	vRemoveBorderBlobs(img, img, EIGHT);
	
	//Fill the holes
	vFillHoles(img, img, EIGHT);

	//lable blobs
	blobs = iLabelBlobs(img, EIGHT);
	
	//analyse blobs
	if(blobs >= 1 && blobs <= 20)
	{
		blobs++;
		for(uint32_t i = 1; i<blobs; i++){
			vCentroid(img, i, &Xc, &Yc, &M00);
			
			if(M00 > IMP2_OBJ_SURFACE_MIN){ //blob surface larger than minimal value
				//searching for the largest blob
				if(blobsize < M00){ //blobsize larger and obj unknown?
					blobsize = M00;
					
					N11 = dNormalizedCentralMoments(img, i, 1, 1, Xc, Yc, M00);
					N20 = dNormalizedCentralMoments(img, i, 2, 0, Xc, Yc, M00);
					N02 = dNormalizedCentralMoments(img, i, 0, 2, Xc, Yc, M00);

					Q1 = N20 + N02;
					Q2 = pow((N20 - N02),2) + (4*pow(N11,2));

					//DEBUG printf, remove later
					/*
					char array[100] = {'\0'};
					snprintf(array,sizeof(array)-1,"B: %d, C: %d, M00 %d ,Q1: %f, Q2: %f\n",blobs-1,color, M00, Q1, Q2);
					
					int i=0;
					while(array[i] != '\0'){
						USART_SendData(USART2, array[i]);
						while(USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET);
						i++;
					}
					*/
					//END DEBUG
					
					if(Q1 > IMP2_OBJ_SQUARE_Q1_MIN && Q1 < IMP2_OBJ_SQUARE_Q1_MAX)
					{
						if(Q2 < IMP2_OBJ_SQUARE_Q2_MAX)
							object = OBJ_SQUARE;
						else
							object = OBJ_UNKNOWN;
					}
					else if(Q1 > IMP2_OBJ_TRIANGLE_Q1_MIN && Q1 < IMP2_OBJ_TRIANGLE_Q1_MAX){
							object = OBJ_TRIANGLE;
					}
					else if(Q1 > IMP2_OBJ_BAR_Q1_MIN && Q1 < IMP2_OBJ_BAR_Q1_MAX){
						object = OBJ_BAR;
					}
					else
							object = OBJ_UNKNOWN;
				}
			}
		}
	}
	return (enum eOBJECTS) object;
}
